#include <Servo.h>
#include <Wire.h>
#include "SDuFAT.h"

//adresses
int HMC6352Address = 0x42;
int slaveAddress;
#define GPM 0x68 // GPM I2C Address                                        // Global variable
#define MEM_PW 8 //patte d'alimentation SD

//constantes de réglage
int neutreservo=83;
int courseservo=45;
int nbrectrlcap=10;
int tempslissage=100;
int coefflargroute=5;
long largroutemin=20;
long largroutemax=50000;
float pi=3.141592654;
long rmoyterre=6371030;
float VminGPS=1;
float coeffproportionnel=40;
float coeffecart=8;
float dt=1;
float detangleaupres=0.7; //équivalent à un angle de 45°
int reactivite=20;

//Variables globales
byte Address;                                         // Global variable
float Data;    
char chaine[10];                                    // array to hold the data for the ftoa fuction   
int posservo=83;
int WPlatdegres[10];
float WPlatminutes[10];
char WPlatletter[10];
int WPlongdegres[10];
float WPlongminutes[10];
char WPlongletter[10];
long WPvalidate[10];
float targetWPlat;
float targetWPlong;
float previousWPlat;
float previousWPlong;
int targetWP=1;
int selectcap=-1;
float capvent[2];
boolean cgtborbdone=false;
boolean verifstrat=false;
boolean suiviroute=true;
long previousdist;
long cmptcgt=0;

//Objets
Servo servo;  // create servo object to control a servo
 
void setup()
{
  servo.attach(2);  // attaches the servo on pin 2 to the servo object
  //alimentation boussole:
  pinMode(16, OUTPUT);
  digitalWrite(16, LOW);
  pinMode(17, OUTPUT);
  digitalWrite(17, HIGH);
  slaveAddress = HMC6352Address >> 1;   // This results in 0x21 as the address to pass to TWI
  Wire.begin();
  // on my MicroSD Module the power comes from a digital pin
  // I activate it at all times
  pinMode(MEM_PW, OUTPUT);
  digitalWrite(MEM_PW, HIGH);
  
  Serial.begin(9600);
  
   //Liste des waypoints:
  //stade
  //WP1:
//  WPlatdegres[0]=48;
//  WPlatminutes[0]=25.135;
//  WPlatletter[0]='N';
//  WPlongdegres[0]=4;
//  WPlongminutes[0]=28.441;
//  WPlongletter[0]='W';
//  WPvalidate[0]=15;
//  //WP2:
//  WPlatdegres[1]=48;
//  WPlatminutes[1]=25.082;
//  WPlatletter[1]='N';
//  WPlongdegres[1]=4;
//  WPlongminutes[1]=28.432;
//  WPlongletter[1]='W';
//  WPvalidate[1]=15;
//  //WP3:
//  WPlatdegres[2]=48;
//  WPlatminutes[2]=25.135;
//  WPlatletter[2]='N';
//  WPlongdegres[2]=4;
//  WPlongminutes[2]=28.441;
//  WPlongletter[2]='W';
//  WPvalidate[2]=15;

//Ty-Colo:
// WPlatdegres[0]=48;
//  WPlatminutes[0]=25.873;
//  WPlatletter[0]='N';
//  WPlongdegres[0]=4;
//  WPlongminutes[0]=36.966;
//  WPlongletter[0]='W';
//  WPvalidate[0]=15;
//  //WP2:
//  WPlatdegres[1]=48;
//  WPlatminutes[1]=25.880;
//  WPlatletter[1]='N';
//  WPlongdegres[1]=4;
//  WPlongminutes[1]=36.762;
//  WPlongletter[1]='W';
//  WPvalidate[1]=15;

//Moulin Blanc
  //WP1:
  WPlatdegres[0]=48;
  WPlatminutes[0]=23.739;
  WPlatletter[0]='N';
  WPlongdegres[0]=4;
  WPlongminutes[0]=25.990;
  WPlongletter[0]='W';
  WPvalidate[0]=40;
  //WP2:
  WPlatdegres[1]=48;
  WPlatminutes[1]=23.818;
  WPlatletter[1]='N';
  WPlongdegres[1]=4;
  WPlongminutes[1]=25.708;
  WPlongletter[1]='W';
  WPvalidate[1]=40;
  //WP6:
  WPlatdegres[2]=48;
  WPlatminutes[2]=23.734;
  WPlatletter[2]='N';
  WPlongdegres[2]=4;
  WPlongminutes[2]=26.054;
  WPlongletter[2]='W';
  WPvalidate[2]=40;
//   //WP3:
//  WPlatdegres[2]=48;
//  WPlatminutes[2]=23.797;
//  WPlatletter[2]='N';
//  WPlongdegres[2]=4;
//  WPlongminutes[2]=25.408;
//  WPlongletter[2]='W';
//  WPvalidate[2]=70; 
//  //WP4:
//  WPlatdegres[2]=48;
//  WPlatminutes[2]=23.732;
//  WPlatletter[2]='N';
//  WPlongdegres[2]=4;
//  WPlongminutes[2]=25.423;
//  WPlongletter[2]='W';
//  WPvalidate[2]=70; 
//  //WP5:
//  WPlatdegres[2]=48;
//  WPlatminutes[2]=23.738;
//  WPlatletter[2]='N';
//  WPlongdegres[2]=4;
//  WPlongminutes[2]=25.767;
//  WPlongletter[2]='W';
//  WPvalidate[2]=40;
}

int GetDouble(){				      // Get double register value from GPM

  int Value = 0; 
  byte H_Byte = 0;
  byte L_Byte = 0;

  Wire.beginTransmission(GPM);
  Wire.send(Address);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(GPM, 2);			      // Request double register
  while(Wire.available() < 2);			      // Wait for double byte
  H_Byte = Wire.receive();                            // Store one
  L_Byte = Wire.receive();                            // Store two

  Value = (H_Byte * 10) + L_Byte;                     // Adjust for one byte

  return(Value);	                          
}


int GetSingle(){				      // Get single register value from GPM

  int Value = 0; 

  Wire.beginTransmission(GPM);
  Wire.send(Address);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(GPM, 1);			      // Request register
  while(Wire.available() < 1);			      // Wait for single byte
  Value = Wire.receive();                             // and store

  return(Value);	                          
}

void GPSHeading(float* cap){ //renvoie le cos et le sin du cap
  float heading;
  Address = 44;                                       // Point to Heading hundreds and tens
  Data = GetDouble();                                 // Read registers from GPM
  heading=10*Data;
  Address = 46;                                       // Point to Heading units
  Data = GetSingle();                                 // Read registers from GPM
  heading=heading+Data;
  Address = 47;                                       // Point to Heading tenths
  Data = GetSingle();                               
  heading=heading+(Data/10);
//  Serial.print("cap GPS ");
//  Serial.println(heading); 
  //  SD.print("log.txt","\nCap GPS: ");
    //SD.println("log.txt", ftoa(chaine, heading, 2));
  heading = radians(heading);
  cap[0]= cos(heading);
  cap[1]= sin(heading);
}

float GPSSpeed(){ //renvoie la vitesse sur un flottant
  float vitesse;
  Address = 52;                                       // Point to Speed hundreds and tens
  Data = GetDouble();                                 // Read registers from GPM
  vitesse=Data*10;
  Address = 54;                                       // Point to Speed units
  Data = GetSingle();                                 // Read registers from GPM
  vitesse=vitesse+Data;
  Address = 55;                                       // Point to Speed units
  Data = GetSingle();                                 // Read registers from GPM
  vitesse=vitesse+(Data/10);
  return vitesse;
}

float GPSLatitude(){ //renvoie la latitude en radians dans notre système (-pi/2 ; pi/2)
  float latitude;
  float minutes;
  char letter;
  Address = 14;                                       // Point to Latitude tens
  Data = GetSingle();                                 // Read registers from GPM
  latitude= Data*10;
  Address = 15;                                       // Point to Latitude units
  Data = GetSingle();                                 // Read registers from GPM
  latitude= latitude+Data;
  Address = 16;                                       // Point to Latitude minutes tens
  Data = GetSingle();                                 // Read registers from GPM
  minutes= Data*10;
  Address = 17;                                       // Point to Latitude minutes units
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data;
  Address = 18;                                       // Point to Latitude minutes tenths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10;
  Address = 19;                                       // Point to Latitude minutes hundredths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/100;
  Address = 20;                                       // Point to Latitude minutes thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/1000;
  Address = 21;                                       // Point to Latitude minutes ten thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10000;
  latitude=latitude+(minutes/60);
  Address = 22;                                       // Point to Latitude character register
  Data = GetSingle();                                 // Read registers from GPM
  letter=Data;
  if (letter=='S'){                 // Prise en conpte de la lettre et passage dans notre système
    latitude=-latitude;
  }
  return radians(latitude);
}

float GPSLongitude(){  //renvoie la longitude en radians dans notre système (-pi ; pi)
  float longitude;
  float minutes;
  char letter;
  Address = 23;                                       // Point to Longitude hundreds
  Data = GetSingle();                                 // Read registers from GPM
  longitude= Data*100;
  Address = 24;                                       // Point to Longitude tens
  Data = GetSingle();                                 // Read registers from GPM
  longitude= longitude+Data*10;
  Address = 25;                                       // Point to Longitude units
  Data = GetSingle();                                 // Read registers from GPM
  longitude= longitude+Data;
  Address = 26;                                       // Point to Longitude minutes tens
  Data = GetSingle();                                 // Read registers from GPM
  minutes= Data*10;
  Address = 27;                                       // Point to Longitude minutes units
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data;
  Address = 28;                                       // Point to Longitude minutes tenths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10;
  Address = 29;                                       // Point to Longitude minutes hundredths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/100;
  Address = 30;                                       // Point to Longitude minutes thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/1000;
  Address = 31;                                       // Point to Longitude minutes ten thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10000;
  longitude=longitude+(minutes/60);
  Address = 32;                                       // Point to Longitude character register
  Data = GetSingle();                                 // Read registers from GPM
  letter=Data;
  if (letter=='E'){                                    // Prise en conpte de la lettre et passage dans notre système
    longitude=-longitude;
  }
  return radians(longitude);
}


void Boussole(float* heading)
{
  byte headingData[2];
  int i;
  float headingValue;
  Wire.beginTransmission(slaveAddress);
  Wire.send("A");              // The "Get Data" command
  Wire.endTransmission();
  delay(10);                   // The HMC6352 needs at least a 70us (microsecond) delay
  // after this command.  Using 10ms just makes it safe
  // Read the 2 heading bytes, MSB first
  // The resulting 16bit word is the compass heading in 10th's of a degree
  // For example: a heading of 1345 would be 134.5 degrees
  Wire.requestFrom(slaveAddress, 2);        // Request the 2 byte heading (MSB comes first)
  i = 0;
  while(Wire.available() && i < 2)
  { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  // Put the MSB and LSB together
  headingValue = headingValue/ 10; //cap sur 360°
  headingValue=360-headingValue;
//  Serial.print("cap boussole ");
//  Serial.println(headingValue); 
  headingValue = radians(headingValue);
  heading[0]= cos(headingValue);
  heading[1]= sin(headingValue);
//Serial.println(heading[0]);
//Serial.println(heading[1]);
} 


void BoussoleLissee(float* caplisse){
  float cap[2];
  float sinus[20];   //nbre pts lissage
  float cosinus[20];  //nbre pts lissage
  int nbrepointslissage=20; //nbre pts lissage DOIT ETRE EGAL AUX VALEURS DANS LES TABLEAUX
  float sommecos=0;
  float sommesin=0;
  
  for (int i=0; i<nbrepointslissage; i++){//boucle pendant laquelle on prélève un cap tous les tempslissage; on prélève nbrepointslissage valeurs.
    Boussole(cap);
    cosinus[i]=cap[0];
    sinus[i]=cap[1];
    delay(tempslissage);
   }
  for (int i=0; i<nbrepointslissage; i++){//somme de tous les caps prélevés ci-dessus
    sommecos=sommecos+cosinus[i];
    sommesin=sommesin+sinus[i];
   } 
  caplisse[0] = sommecos/nbrepointslissage;
  caplisse[1] = sommesin/nbrepointslissage;
//Serial.println(caplisse[0]);
//Serial.println(caplisse[1]);
//    SD.print("log.txt","\nCap boussole lissee cos: ");
//    SD.println("log.txt", ftoa(chaine, caplisse[0], 2));
//        SD.print("log.txt","\nsin: ");
//    SD.println("log.txt", ftoa(chaine, caplisse[1], 2));
 }


char *ftoa(char *a, double f, int precision) {
  long p[] = {0,10,100,1000,10000,100000,1000000,10000000,100000000};
  
  char *ret = a;
  long heiltal = (long)f;
  itoa(heiltal, a, 10);
  while (*a != '\0') a++;
  *a++ = '.';
  long desimal = abs((long)((f - heiltal) * p[precision]));
  itoa(desimal, a, 10);
  return ret;
} 

long dist(float latitudedep, float longitudedep, float latitudear, float longitudear){ // Retourne la distance en mètres entre 2 points GPS avec une erreur maximale de 0.2%
  float latitude;
  float longitude;
  float a;
  float c;
  float d;
  float e;
  long distance;
  latitude= (abs(latitudedep-latitudear));
  longitude= min(min(abs(longitudedep-longitudear), abs((2*pi+longitudedep)-longitudear)), abs(longitudedep-(longitudear+2*pi)));
  a = (square(sin(latitude/2.0))) + cos(latitudedep)*cos(latitudear)*(square(sin(longitude/2.0)));
  d=sqrt(a);
  e=sqrt(1-a);
  c = 2*atan2(d, e);
  distance= rmoyterre*c;
  return distance;
}

void GPSHeadingTh(float latitudedep, float longitudedep, float latitudear, float longitudear, float* cap){  // Retourne le cos le sin du cap entre 2 points GPS
  float heading;
  float a;
  float b;
  float c;
  float d;
  float longitude;
  longitude= min(min(abs(longitudedep-longitudear), abs((2*pi+longitudedep)-longitudear)), abs(longitudedep-(longitudear+2*pi)));
  a=sin(longitude)*cos(latitudear);
  c=cos(latitudedep)*sin(latitudear);
  d=sin(latitudedep)*cos(latitudear)*cos(longitude);
  b=c-d;
  heading=atan2(a,b);
  if (sin(longitudear-longitudedep)>0){                    //passage de [-180°; 180°] à [0°; 360°] pour rejoindre la convention des caps d'un compas.
   heading=2*pi-abs(heading); 
  }
  cap[0]=cos(heading);
  cap[1]=sin(heading);
}


float ConversionLatitude(int degres, float minutes, char letter){
  float latitude;
  latitude= degres+ minutes/60;
  if (letter=='S'){                                      // Prise en conpte de la lettre et passage dans notre système
    latitude=-latitude;
  }
  return radians(latitude);
}

float ConversionLongitude(int degres, float minutes, char letter){
  float longitude;
  longitude= degres+ minutes/60;
  if (letter=='E'){                                    // Prise en conpte de la lettre et passage dans notre système
    longitude=-longitude;
  }
  return radians(longitude);
}

void servocmd(int angle){                     //verifie que l'on ne depasse pas les butées de commande du servo
  if (angle<=neutreservo+courseservo && angle>=neutreservo-courseservo){
    servo.write(neutreservo-(angle-neutreservo));
  }
  else{
    if(angle>neutreservo+courseservo){
      servo.write(neutreservo-courseservo);
    }
    if(angle<neutreservo-courseservo){
      servo.write(neutreservo+courseservo);
    }
  }
   //Serial.print("Angle servo:");
   //Serial.println(angle);
     //  SD.print("log.txt","\nangle servo: ");
 //   SD.println("log.txt", ftoa(chaine, angle, 2));
   
}

float ecartroute(float latitudedep, float longitudedep, float latitudear, float longitudear){ //retourne l'écart à la route définie par les 2 points GPS en m sur un long
float headingroute[2];
GPSHeadingTh( latitudedep, longitudedep, latitudear, longitudear, headingroute);
float proj[2];
proj[0]=-headingroute[1];
proj[1]=headingroute[0];
float headingtocurrentpoint[2];
GPSHeadingTh(latitudedep, longitudedep, GPSLatitude(), GPSLongitude(), headingtocurrentpoint);
float distancedone=dist(latitudedep, longitudedep, GPSLatitude(), GPSLongitude());        //distance parcourue
float ecart;
ecart= distancedone*(proj[0]*headingtocurrentpoint[0]+proj[1]*headingtocurrentpoint[1]);
   //Serial.print("Ecart: ");
   //Serial.println(ecart);
   //    SD.print("log.txt","\nEcart: ");
   // SD.println("log.txt", ftoa(chaine, ecart, 2));
return ecart;
}


boolean SuiviRoute(float latitudedep, float longitudedep, float latitudear, float longitudear){  
  //Serial.println("Suivi route");
     // SD.print("log.txt","\nSuivi route ");
  //calcul des variables nécessaires pour la fonction:
  long longroute= dist(latitudedep, longitudedep, latitudear, longitudear);
  long largroute= longroute/coefflargroute;
  float ecart;
  float theoricalheading[2];
  GPSHeadingTh( GPSLatitude(), GPSLongitude(), latitudear, longitudear, theoricalheading);
  float currentheading[2];
  float capboussole[2];
  float erreur;
 //Calcul de la largeur de la route
 if (largroute>largroutemax){
   largroute=largroutemax;
 }
  if (largroute<largroutemin){
   largroute=largroutemin;
 }
 //Vérification que le bateau est bien présent à l'intérieur du couloir de la route définie, calcul de l'écart à la route:
 ecart=ecartroute(latitudedep, longitudedep, latitudear, longitudear);  //ecart à la route en mètres
 if(abs(ecart)<largroute/2){       //bateau présent dans le couloir 
   ecart=ecart/largroute;          //ecart relatif prenant en compte la largeur de la route, varie de -1 à 1 sur la route.
   if (GPSSpeed()<=VminGPS){ // asservir sur cap boussole.
     BoussoleLissee(capboussole);
     if (capboussole[0]*theoricalheading[0]+capboussole[1]*theoricalheading[1]>0){
       erreur=capboussole[0]*theoricalheading[1]-capboussole[1]*theoricalheading[0];    //determinant donnant une image de l'erreur de cap, varie de -1 à 1 dans le sens de marche
       servocmd(neutreservo+(-coeffproportionnel*erreur+coeffecart*ecart));   //commande du servo double propotionnelle et intégrale
       //Serial.println("asservi sur boussole");
     // SD.print("log.txt","\nasservi sur boussole ");
     }
     else{                           //si on se trouve à l'envers du sens de marche
       if(capboussole[0]*theoricalheading[1]-capboussole[1]*theoricalheading[0]>0){
         servocmd(neutreservo-courseservo);       //virer sur tribord
         //Serial.println("virer sur tribord boussole");
             //  SD.print("log.txt","\nvirer sur tribord boussole ");
       }
       else{
         servocmd(neutreservo+courseservo);       //virer sur babord  
         //Serial.println("virer sur babord boussole");
            //   SD.print("log.txt","\nvirer sur babord boussole ");
       }
     }
   }
   else{ // asservir sur cap GPS
     GPSHeading(currentheading);
     if (currentheading[0]*theoricalheading[0]+currentheading[1]*theoricalheading[1]>0){
       erreur=currentheading[0]*theoricalheading[1]-currentheading[1]*theoricalheading[0];//determinant donnant une image de l'erreur de cap, varie de -1 à 1 dans le sens de marche
       servocmd(neutreservo+(-coeffproportionnel*erreur+coeffecart*ecart));   //commande du servo double propotionnelle et intégrale
       //Serial.println("asservi sur gps");
           //  SD.print("log.txt","\nasservi sur gps ");
     }
     else{                    //si on se trouve à l'envers du sens de marche
       if(currentheading[0]*theoricalheading[1]-currentheading[1]*theoricalheading[0]>0){
         servocmd(neutreservo-courseservo);       //virer sur tribord
         //Serial.println("virer sur tribord gps");
              // SD.print("log.txt","\nvirer sur tribord gps ");
       }
       else{
         servocmd(neutreservo+courseservo);       //virer sur babord
         //Serial.println("virer sur babord gps");  
          //     SD.print("log.txt","\virer sur babord gps ");
       }
     }
   } 
  return true;  
 }
 //Serial.println("Impossible");
      // SD.print("log.txt","\nImpossible ");
 return false;
}

boolean remontervent(float latitudedep, float longitudedep, float latitudear, float longitudear){ 
  //Serial.println("Remonter vent");
   //SD.print("log.txt","\nRemonter vent ");  
  //calcul des variables nécessaires pour la fonction:
  long longroute= dist(latitudedep, longitudedep, latitudear, longitudear);
  long largroute= longroute/coefflargroute;
  float ecart;
  float currentheading[2];
  float capboussole[2];
  float erreur;
 //Calcul de la largeur de la route
 if (largroute>largroutemax){
   largroute=largroutemax;
 }
  if (largroute<largroutemin){
   largroute=largroutemin;
 }
 //Vérification que le bateau est bien présent à l'intérieur du couloir de la route définie, calcul de l'écart à la route:
 ecart=ecartroute(latitudedep, longitudedep, latitudear, longitudear);  //ecart à la route en mètres
 if(abs(ecart)<largroute/2){       //bateau présent dans le couloir 
   if (abs(ecart)<largroute/4){
     cgtborbdone=false;
   }
   if (abs(ecart)<largroute/3 || cgtborbdone==true){       
     if (GPSSpeed()<=VminGPS){ // asservir sur cap boussole.
       BoussoleLissee(capboussole);
       if (capboussole[0]*capvent[0]+capboussole[1]*capvent[1]>0){
         erreur=-selectcap*detangleaupres+(capboussole[0]*capvent[1]+capboussole[1]*capvent[0]);    //determinant donnant une image de l'erreur de cap, varie de -1 à 1 dans le sens de marche
         //Serial.print("erreur: ");
         //Serial.println(erreur); 
          //SD.print("log.txt","\nerreur:  ");
            //  SD.println("log.txt", ftoa(chaine, erreur, 2));
         servocmd(neutreservo+(coeffproportionnel*erreur));   //commande du servo double propotionnelle et intégrale
         //Serial.println("asservi sur boussole");
         // SD.print("log.txt","\nasservi sur boussole ");
       }
       else{                           //si on se trouve à l'envers du sens de marche
         if(selectcap<0){
           servocmd(neutreservo-courseservo);       //virer sur tribord
           //Serial.println("virer sur tribord boussole");
           // SD.print("log.txt","\nvirer sur tribord boussole ");
         }
         else{
           servocmd(neutreservo+courseservo);       //virer sur babord  
           //Serial.println("virer sur babord boussole");
            //SD.print("log.txt","\nvirer sur babord boussole ");
         }
       }
     }
     else{ // asservir sur cap GPS
       GPSHeading(currentheading);
       if (currentheading[0]*capvent[0]+currentheading[1]*capvent[1]>0){
         erreur=-selectcap*detangleaupres+(currentheading[0]*capvent[1]+currentheading[1]*capvent[0]);    //determinant donnant une image de l'erreur de cap, varie de -1 à 1 dans le sens de marche
         servocmd(neutreservo+(coeffproportionnel*erreur));
         //Serial.println("asservi sur gps");
          //SD.print("log.txt","\nasservi sur gps ");
       }
       else{                    //si on se trouve à l'envers du sens de marche
         if(selectcap<0){
           servocmd(neutreservo-courseservo);       //virer sur tribord
           //Serial.println("virer sur tribord gps");
            //SD.print("log.txt","\nvirer sur tribord gps ");
         }
         else{
           servocmd(neutreservo+courseservo);       //virer sur babord
           //Serial.println("virer sur babord gps"); 
           //SD.print("log.txt","\nvirer sur babord gps "); 
         }
       }
     } 
   }
   else{
     cgtborbdone=true;
     Boussole(capboussole);
     //Serial.println("Changement de bord pour remonter au vent");
     //SD.print("log.txt","\nChangement de bord pour remonter au vent "); 
     if (selectcap<0){
       selectcap=1;
       //Serial.println("virer sur tribord");
       // SD.print("log.txt","\nvirer sur tribord ");
       servocmd(neutreservo-courseservo);       //virer sur tribord
       for (int i=0; i<100; i++){
         int timing=100;
         if(capboussole[0]*capvent[1]+capboussole[1]*capvent[0]>detangleaupres){
           timing=0;
         }
         delay(timing);
       }
     }
     else{
       selectcap=-1;
       //Serial.println("virer sur babord");
       // SD.print("log.txt","\nvirer sur babord ");
       servocmd(neutreservo+courseservo);       //virer sur babord
       for (int i=0; i<100; i++){
         int timing=100;
         if(capboussole[0]*capvent[1]+capboussole[1]*capvent[0]<-detangleaupres){
           timing=0;
         }
         delay(timing);
       }
     }
     servocmd(neutreservo);  //remise en ligne droite
   }
  return true;  
 }
 //Serial.println("Impossible");
  //SD.print("log.txt","\nImpossible ");
 return false;
}



void cgtpreviousWP(){
  //Serial.print("Waypoint precedent devient la position actuelle");
  int latitude;
  float minutes;
  float longitude;
  Address = 14;                                       // Point to Latitude tens
  Data = GetSingle();                                 // Read registers from GPM
  latitude= Data*10;
  Address = 15;                                       // Point to Latitude units
  Data = GetSingle();                                 // Read registers from GPM
  latitude= latitude+Data;
  Address = 16;                                       // Point to Latitude minutes tens
  Data = GetSingle();                                 // Read registers from GPM
  minutes= Data*10;
  Address = 17;                                       // Point to Latitude minutes units
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data;
  Address = 18;                                       // Point to Latitude minutes tenths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10;
  Address = 19;                                       // Point to Latitude minutes hundredths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/100;
  Address = 20;                                       // Point to Latitude minutes thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/1000;
  Address = 21;                                       // Point to Latitude minutes ten thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10000;
  Address = 22;                                       // Point to Latitude character register
  Data = GetSingle();                                 // Read registers from GPM
  WPlatletter[targetWP-1]=Data; 
  WPlatdegres[targetWP-1]=latitude;
  WPlatminutes[targetWP-1]=minutes;

  Address = 23;                                       // Point to Longitude hundreds
  Data = GetSingle();                                 // Read registers from GPM
  longitude= Data*100;
  Address = 24;                                       // Point to Longitude tens
  Data = GetSingle();                                 // Read registers from GPM
  longitude= longitude+Data*10;
  Address = 25;                                       // Point to Longitude units
  Data = GetSingle();                                 // Read registers from GPM
  longitude= longitude+Data;
  Address = 26;                                       // Point to Longitude minutes tens
  Data = GetSingle();                                 // Read registers from GPM
  minutes= Data*10;
  Address = 27;                                       // Point to Longitude minutes units
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data;
  Address = 28;                                       // Point to Longitude minutes tenths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10;
  Address = 29;                                       // Point to Longitude minutes hundredths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/100;
  Address = 30;                                       // Point to Longitude minutes thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/1000;
  Address = 31;                                       // Point to Longitude minutes ten thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10000;
  Address = 32;                                       // Point to Longitude character register
  Data = GetSingle();                                 // Read registers from GPM
  WPlongletter[targetWP-1]=Data;
  WPlongdegres[targetWP-1]=longitude;
  WPlongminutes[targetWP-1]=minutes;
}

void navigation(){
  previousWPlat=ConversionLatitude(WPlatdegres[targetWP-1], WPlatminutes[targetWP-1], WPlatletter[targetWP-1]);
  previousWPlong=ConversionLongitude(WPlongdegres[targetWP-1], WPlongminutes[targetWP-1], WPlongletter[targetWP-1]);
  targetWPlat=ConversionLatitude(WPlatdegres[targetWP], WPlatminutes[targetWP], WPlatletter[targetWP]);
  targetWPlong=ConversionLongitude(WPlongdegres[targetWP], WPlongminutes[targetWP], WPlongletter[targetWP]);
  if (dist(GPSLatitude(), GPSLongitude(), targetWPlat, targetWPlong)<WPvalidate[targetWP]){    //distance de validation waypoint atteinte
    //Serial.print("Changement de waypoint");
    targetWP++; 
    previousWPlat=ConversionLatitude(WPlatdegres[targetWP-1], WPlatminutes[targetWP-1], WPlatletter[targetWP-1]);
    previousWPlong=ConversionLongitude(WPlongdegres[targetWP-1], WPlongminutes[targetWP-1], WPlongletter[targetWP-1]);
    targetWPlat=ConversionLatitude(WPlatdegres[targetWP], WPlatminutes[targetWP], WPlatletter[targetWP]);
    targetWPlong=ConversionLongitude(WPlongdegres[targetWP], WPlongminutes[targetWP], WPlongletter[targetWP]);
  }
}

void strategie(){
  if (verifstrat==false){
    cgtpreviousWP();
    verifstrat=true;
    suiviroute=true;
  }
  navigation();
  GPSHeadingTh(previousWPlat, previousWPlong, targetWPlat, targetWPlong, capvent);
  previousdist=dist(GPSLatitude(), GPSLongitude(), targetWPlat, targetWPlong);
  if (suiviroute==true){
    verifstrat=SuiviRoute(previousWPlat, previousWPlong, targetWPlat, targetWPlong);
  }
  else{
    verifstrat=remontervent(previousWPlat, previousWPlong, targetWPlat, targetWPlong);
  }
  delay(1000);
  cmptcgt++;
  if (previousdist<dist(GPSLatitude(), GPSLongitude(), targetWPlat, targetWPlong) && cmptcgt>reactivite){
    cmptcgt=0;
    if(suiviroute==true){
      suiviroute=false;
    }
    else{
      suiviroute=true;
    }
  }
}

void loop(){
//  Serial.print("Speed: ");                               //test vitesse GPS
//  Serial.println(GPSSpeed());
//  Serials.print("100000*longitude: ");                    // test longitude GPS
//  Serial.println(100000*GPSLongitude());
//  Serial.print("100000*latitude: ");                     // test latitude GPS
//  Serial.println(100000*GPSLatitude());
//  Serial.println(1000*ConversionLongitude(45, 25.2356, 'E')); //Test conversion coordonnées GPS
//  Serial.println(1000*ConversionLatitude(45, 25.2356, 'N'));
//  Serial.println(dist(ConversionLatitude(48, 22.666, 'N'), ConversionLongitude(4, 29.628, 'O'), ConversionLatitude(48, 18.701, 'N'), ConversionLongitude(4, 29.947, 'O'))); //test distance
// float a[2];
// GPSHeadingTh(ConversionLatitude(WPlatdegres[0], WPlatminutes[0], WPlatletter[0]), ConversionLongitude(WPlongdegres[0], WPlongminutes[0], WPlongletter[0]), ConversionLatitude(WPlatdegres[1], WPlatminutes[1], WPlatletter[1]),  ConversionLongitude(8, WPlongminutes[1], WPlongletter[1]),a);
//SuiviRoute(ConversionLatitude(WPlatdegres[0], WPlatminutes[0], WPlatletter[0]), ConversionLongitude(WPlongdegres[0], WPlongminutes[0], WPlongletter[0]), ConversionLatitude(WPlatdegres[1], WPlatminutes[1], WPlatletter[1]),  ConversionLongitude(WPlongdegres[1], WPlongminutes[1], WPlongletter[1]));
//delay(3000);
//float a[2];
//GPSHeading(a);
//test cgtprevwaypoint
//targetWP=1;
//cgtpreviousWP();
//   Serial.println(10*WPlatdegres[0]);
//   Serial.println(10*WPlatminutes[0]);
//   Serial.println(WPlatletter[0]);
//   Serial.println(10*WPlongdegres[0]);
//   Serial.println(10*WPlongminutes[0]);
//   Serial.println(WPlongletter[0]);
//Remonter au vent

//capvent[0]=-1;
//capvent[1]=0;
//Serial.println("selectcap = ");
//Serial.println(selectcap);
//remontervent(ConversionLatitude(WPlatdegres[0], WPlatminutes[0], WPlatletter[0]), ConversionLongitude(WPlongdegres[0], WPlongminutes[0], WPlongletter[0]), ConversionLatitude(WPlatdegres[1], WPlatminutes[1], WPlatletter[1]),  ConversionLongitude(WPlongdegres[1], WPlongminutes[1], WPlongletter[1]));
//targetWP=1;
//navigation();
//Serial.println(targetWP);
strategie();
}
